#include "sr.h"

#include <stdio.h>

static int state;

static char inEmMode;
static double posdesired;
static double poscar;
static int count;

static double q[5];

static double K[5] = { +1.0906e+03, -3.2981e+03, +5.7106e+00, +0.0000e+00, +3.0647e+03 };

static double A[5][5] = { 
    { +9.9950e-01, +9.8040e-01, +9.9444e-04, -2.9370e+01, +4.9032e-03 },
    { +4.9882e-07, +9.9892e-01, -9.9632e-07, +4.8569e+01, +9.9963e-03 },
    { +0.0000e+00, +0.0000e+00, +9.9004e-01, -5.2731e+04, +0.0000e+00 },
    { +9.9975e-03, +4.9033e-03, +4.9817e-06, +9.9757e-02, +1.6346e-05 },
    { +9.9739e-05, -2.1569e-01, -1.9888e-04, -9.5679e+01, +9.9892e-01} 
};
    
static double B[5][5] = {
    { +1.9926e-05, +2.9370e+01 },
    { -1.3296e-08, -4.8569e+01 },
    { +3.9800e-02, +5.2731e+04 },
    { +6.6485e-08, +9.0024e-01 },
    { -3.9853e-06, +9.5679e+01 }
};

static double kp = +5.5000e+02;

#define READY  1
#define RUNNING 2
#define WAITING 3
#define ZEROVC 4

void
initControlAlg(void) {
    int i;

    state = READY;
    inEmMode = 0;
    count = 0;
    posdesired = -10.0;
    for(i = 0;i < 5;i++) {
        q[i] = 0.0;
    }
}

void
controlAlgFire(double Posdesired, double poscar, double alfa, int hasEventFlags, 
        double *vc, int *craneKnownFlags, int *cranePresentFlags) {

    double y, z, u, qnext[5];
    int i, j;

    if(GET_BIT(*craneKnownFlags, EMMODECHECKALFABIT)) {
        if(GET_BIT(*cranePresentFlags, EMMODECHECKALFABIT)) {
            inEmMode = 1;
        }
    }

    if(GET_BIT(hasEventFlags, POSDESIREDBIT)) {
        posdesired = Posdesired;
    }

    if(state == ZEROVC) {
        SET_BIT_TRUE(*craneKnownFlags, VCCONTROLALGBIT);
        SET_BIT_TRUE(*cranePresentFlags, VCCONTROLALGBIT);
        *vc = 0.0;
    }
    else
    if(state == RUNNING) {
        if(GET_BIT(*craneKnownFlags, EMSTOPMERGERBIT)) {
            if(GET_BIT(*cranePresentFlags, EMSTOPMERGERBIT)) {
                *vc = 0.0;
                SET_BIT_TRUE(*craneKnownFlags, VCCONTROLALGBIT);
                SET_BIT_TRUE(*cranePresentFlags, VCCONTROLALGBIT);
            }
            else
            if(GET_BIT(hasEventFlags, SHUTDOWNBIT)) {
                *vc = 0.0;
                SET_BIT_TRUE(*craneKnownFlags, VCCONTROLALGBIT);
                SET_BIT_TRUE(*cranePresentFlags, VCCONTROLALGBIT);
            }
            else
            if(GET_BIT(hasEventFlags, XCBIT)) {
                count++;
                if(count == 5) {
                    count = 0;

                    y = 0.0;
                    for(i = 0;i < 5;i++) {
                        y = y + K[i] * q[i];
                    }

                    if(inEmMode) {
                        z = q[1];
                    }
                    else {
                        z = alfa;
                    }

                    z = poscar + 0.5 * z;
                    u = kp * (posdesired - z);
                    u = u - y;

                    if(u > 40.0) {
                        *vc = 40.0;
                    }
                    else
                    if(u < -40.0) {
                        *vc = -40.0;
                    }
                    else {
                        *vc = u;
                    }

                    SET_BIT_TRUE(*craneKnownFlags, VCCONTROLALGBIT);
                    SET_BIT_TRUE(*cranePresentFlags, VCCONTROLALGBIT);

                    for(i = 0;i < 5;i++) {
                        qnext[i] = B[i][0] * (*vc) + B[i][1] * poscar;
                        for(j = 0;j < 5;j++) {
                            qnext[i] = qnext[i] + A[i][j] * q[j];
                        }
                    }
                    for(i = 0;i < 5;i++) q[i] = qnext[i];
                }
            }
        }
    }

    if(GET_BIT(*craneKnownFlags, EMSTOPMERGERBIT) && 
            !GET_BIT(*craneKnownFlags, VCCONTROLALGBIT)) {
        SET_BIT_TRUE(*craneKnownFlags, VCCONTROLALGBIT);
    }

}

void
controlAlgPostfire(char brake, int hasEventFlags, int craneKnownFlags, 
        int cranePresentFlags) {

    switch(state) {
        case READY: {
            if(GET_BIT(craneKnownFlags, TRIGGERBIT)) {
                if(GET_BIT(cranePresentFlags, TRIGGERBIT)) {
                    if(GET_BIT(hasEventFlags, POSDESIREDBIT)) {
                        state = RUNNING;
                    }
                    else {
                        state = WAITING;
                    }
                }
            }
        } break;

        case WAITING: {
            if(GET_BIT(hasEventFlags, SHUTDOWNBIT)) {
                initControlAlg();
                break;
            }

            if(GET_BIT(craneKnownFlags, EMSTOPMERGERBIT)) {
                if(GET_BIT(cranePresentFlags, EMSTOPMERGERBIT)) {
                    initControlAlg();
                    break;
                }
            }

            if(GET_BIT(hasEventFlags, POSDESIREDBIT)) {
                state = RUNNING;
            }
        } break;

        case RUNNING: {
            if(GET_BIT(hasEventFlags, SHUTDOWNBIT)) {
                initControlAlg();
                break;
            }
            
            if(GET_BIT(craneKnownFlags, EMSTOPMERGERBIT)) {
                if(GET_BIT(cranePresentFlags, EMSTOPMERGERBIT)) {
                    initControlAlg();
                    break;
                }
            }

            if(GET_BIT(craneKnownFlags, BRAKEBIT)) {
                if(GET_BIT(cranePresentFlags, BRAKEBIT) && 
                    !GET_BIT(cranePresentFlags, EMSTOPMERGERBIT)) {
                    if(brake == 1) {
                        posdesired = -10.0;
                        count = 0;
                        state = ZEROVC;
                    }
                }
            }
        } break;
        
        case ZEROVC : {
            if(GET_BIT(hasEventFlags, SHUTDOWNBIT)) {
                initControlAlg();
                break;
            }

            if(GET_BIT(craneKnownFlags, EMSTOPMERGERBIT)) {
                if(GET_BIT(cranePresentFlags, EMSTOPMERGERBIT)) {
                    initControlAlg();
                    break;
                }
            }

            if(GET_BIT(hasEventFlags, POSDESIREDBIT)) {
                state = RUNNING;
            }
            else {
                state = WAITING;
            }
        } break;
    }
}
